function [var e1 e2 e3 e4 c1 c2 c3 Q q] = Generate_Random_Data_Full(rotation_type,point_config)
%Output:
%var = [a b c d]; ground_truth
%e1,e2,e3,e4: the coefficients of polynomials of our formulation
%c1,c2,c3: the coefficients of polynomials of DLS
%Q,q: the objective function used in polishing

%generate ground_truth rotations
if strcmp(rotation_type,'Fully_Random')
    temp = randn(1,4);
elseif strcmp(rotation_type,'Near_Cayley_Degenerate')
    temp = randn(1,4);
    temp(1) = 1e-4; %a small value,near degenerate to Cayley
elseif strcmp(rotation_type,'Cayley_Degenerate')
    temp = randn(1,4);
    temp(1) = 0; % degenerate to Cayley
end

%Ground-truth quaternions
temp = temp/norm(temp);

a = temp(1); b = temp(2); c = temp(3); d = temp(4);
var = [a b c d];

%Ground-truth rotation matrix
R = [a^2+b^2-c^2-d^2     2*b*c-2*a*d     2*b*d+2*a*c
     2*b*c+2*a*d         a^2-b^2+c^2-d^2 2*c*d-2*a*b
     2*b*d-2*a*c         2*c*d+2*a*b   a^2-b^2-c^2+d^2];
 
%number of points
npt = 50;

% camera's parameters
width= 640;
height= 480;
f= 800;

if strcmp(point_config,'Ordinary_3D')
    % generate 3d coordinates in camera space
    Xc= [xrand(1,npt,[-2 2]); xrand(1,npt,[-2 2]); xrand(1,npt,[4 8])];
    t= mean(Xc,2);

    U = inv(R)*(Xc-repmat(t,1,npt));

    % projection
    xx= [Xc(1,:)./Xc(3,:); Xc(2,:)./Xc(3,:)]*f;
    xxn= xx+randn(2,npt)*0; %noise-free
    u = xxn/f;
elseif strcmp(point_config,'Quasi_Singular')
    % generate 3d coordinates in camera space
    Xc= [xrand(1,npt,[1 2]); xrand(1,npt,[1 2]); xrand(1,npt,[4 8])];
    t= mean(Xc,2);
    U= inv(R)*(Xc-repmat(t,1,npt));

    % projection
    xx= [Xc(1,:)./Xc(3,:); Xc(2,:)./Xc(3,:)]*f;
    xxn= xx+randn(2,npt)*0; %noise-free
    u = xxn/f;
else
    % generate 3d coordinates in camera space
    XXw= [xrand(2,npt,[-2 2]); zeros(1,npt)];
    t= [rand-0.5;rand-0.5;rand*8+4];
    Xc= R*XXw+repmat(t,1,npt);
    U = XXw;

    % projection
    xx= [Xc(1,:)./Xc(3,:); Xc(2,:)./Xc(3,:)]*f;
    xxn= xx+randn(2,npt)*0;
    u = xxn/f;
end

%using the fractional formulation
n = npt;

%homogeneous coordinate
if size(u,1) > 2
    u = u(1:2,:);
end

%3D points after translation to centroid
Ucent = mean(U,2);
Um = U - repmat(Ucent,1,n);
xm = Um(1,:)'; ym = Um(2,:)'; zm = Um(3,:)';
x = U(1,:)'; y = U(2,:)'; z = U(3,:)';
u1 = u(1,:)'; v1 = u(2,:)';


%construct matrix N: 2n*11
N = zeros(2*n,11);
N(1:2:end,:) = [ u1, u1.*zm - x, 2*u1.*ym, - 2*z - 2*u1.*xm, 2*y, - x - u1.*zm, -2*y, 2*u1.*xm - 2*z, x - u1.*zm, 2*u1.*ym, x + u1.*zm];
N(2:2:end,:) = [ v1, v1.*zm - y, 2*z + 2*v1.*ym, -2*v1.*xm, -2*x, y - v1.*zm, -2*x, 2*v1.*xm, - y - v1.*zm, 2*v1.*ym - 2*z, y + v1.*zm];
MTN = [sum(N(1:2:end,:)); sum(N(2:2:end,:))];

%construct matrix Q: 11*11
Q = N'*N - 1/n*(MTN')*MTN;
q = Q(1,2:end);
Q = Q(2:end,2:end);

Q11 = Q(1,1); Q12 = Q(1,2); Q13 = Q(1,3); Q14 = Q(1,4); Q15 = Q(1,5); Q16 = Q(1,6); Q17 = Q(1,7); Q18 = Q(1,8); Q19 = Q(1,9); Q110 = Q(1,10); 
              Q22 = Q(2,2); Q23 = Q(2,3); Q24 = Q(2,4); Q25 = Q(2,5); Q26 = Q(2,6); Q27 = Q(2,7); Q28 = Q(2,8); Q29 = Q(2,9); Q210 = Q(2,10); 
                            Q33 = Q(3,3); Q34 = Q(3,4); Q35 = Q(3,5); Q36 = Q(3,6); Q37 = Q(3,7); Q38 = Q(3,8); Q39 = Q(3,9); Q310 = Q(3,10); 
                                          Q44 = Q(4,4); Q45 = Q(4,5); Q46 = Q(4,6); Q47 = Q(4,7); Q48 = Q(4,8); Q49 = Q(4,9); Q410 = Q(4,10); 
                                                        Q55 = Q(5,5); Q56 = Q(5,6); Q57 = Q(5,7); Q58 = Q(5,8); Q59 = Q(5,9); Q510 = Q(5,10);
                                                                      Q66 = Q(6,6); Q67 = Q(6,7); Q68 = Q(6,8); Q69 = Q(6,9); Q610 = Q(6,10);
                                                                                    Q77 = Q(7,7); Q78 = Q(7,8); Q79 = Q(7,9); Q710 = Q(7,10);
                                                                                                  Q88 = Q(8,8); Q89 = Q(8,9); Q810 = Q(8,10);
                                                                                                                Q99 = Q(9,9); Q910 = Q(9,10);
                                                                                                                              Q1010 = Q(10,10);
q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5); q6 = q(6); q7 = q(7); q8 = q(8); q9 = q(9); q10 = q(10);

%variable sequence
%[ a^3, a^2*b, a^2*c, a^2*d, a*b^2, a*b*c, a*b*d, a*c^2, a*c*d, a*d^2, a, b^3, b^2*c, b^2*d, b*c^2, b*c*d, b*d^2, b, c^3, c^2*d, c*d^2, c, d^3, d]

e1 = [ 4*Q11, 6*Q12, 6*Q13, 6*Q14, 4*Q15 + 2*Q22, 4*Q16 + 4*Q23, 4*Q17 + 4*Q24, 4*Q18 + 2*Q33, 4*Q19 + 4*Q34, 4*Q110 + 2*Q44, 4*q1, 2*Q25, 2*Q26 + 2*Q35, 2*Q27 + 2*Q45, 2*Q28 + 2*Q36, 2*Q29 + 2*Q37 + 2*Q46, 2*Q210 + 2*Q47, 2*q2, 2*Q38, 2*Q39 + 2*Q48, 2*Q310 + 2*Q49, 2*q3, 2*Q410, 2*q4];
e2 = [ 2*Q12, 4*Q15 + 2*Q22, 2*Q16 + 2*Q23, 2*Q17 + 2*Q24, 6*Q25, 4*Q26 + 4*Q35, 4*Q27 + 4*Q45, 2*Q28 + 2*Q36, 2*Q29 + 2*Q37 + 2*Q46, 2*Q210 + 2*Q47, 2*q2, 4*Q55, 6*Q56, 6*Q57, 4*Q58 + 2*Q66, 4*Q59 + 4*Q67, 4*Q510 + 2*Q77, 4*q5, 2*Q68, 2*Q69 + 2*Q78, 2*Q610 + 2*Q79, 2*q6, 2*Q710, 2*q7];
e3 = [ 2*Q13, 2*Q16 + 2*Q23, 4*Q18 + 2*Q33, 2*Q19 + 2*Q34, 2*Q26 + 2*Q35, 4*Q28 + 4*Q36, 2*Q29 + 2*Q37 + 2*Q46, 6*Q38, 4*Q39 + 4*Q48, 2*Q310 + 2*Q49, 2*q3, 2*Q56, 4*Q58 + 2*Q66, 2*Q59 + 2*Q67, 6*Q68, 4*Q69 + 4*Q78, 2*Q610 + 2*Q79, 2*q6, 4*Q88, 6*Q89, 4*Q810 + 2*Q99, 4*q8, 2*Q910, 2*q9];
e4 = [ 2*Q14, 2*Q17 + 2*Q24, 2*Q19 + 2*Q34, 4*Q110 + 2*Q44, 2*Q27 + 2*Q45, 2*Q29 + 2*Q37 + 2*Q46, 4*Q210 + 4*Q47, 2*Q39 + 2*Q48, 4*Q310 + 4*Q49, 6*Q410, 2*q4, 2*Q57, 2*Q59 + 2*Q67, 4*Q510 + 2*Q77, 2*Q69 + 2*Q78, 4*Q610 + 4*Q79, 6*Q710, 2*q7, 2*Q89, 4*Q810 + 2*Q99, 6*Q910, 2*q9, 4*Q1010, 4*q10];

%call the solver
%[x y z t] = GB_Solver_3Order_4Variable_Division(e1, e2, e3, e4);

%using the unit-norm formulation
% M = zeros(2*n,3);
% M(1:2:end,1) = -1;
% M(2:2:end,2) = -1;
% M(:,3) = u(:);
% 
% N = zeros(2*n,10);
% for i=1:n
%     N(2*(i-1)+1,:) = U(:,i)'*[1       0         2*u(1,i) 0      1       0   -2*u(1,i)   -1      0           -1
%                               0       -2*u(1,i) 0       -2      0       2   0           0       -2*u(1,i)   0
%                               -u(1,i) 0         2       0       u(1,i)  0   2           u(1,i)  0           -u(1,i)];
%               
%     N(2*(i-1)+2,:) = U(:,i)'*[0       0         2*u(2,i) 2      0       2  -2*u(2,i)    0      0            0
%                               1       -2*u(2,i) 0        0      -1      0   0           1       -2*u(2,i)   -1
%                               -u(2,i) -2        0        0      u(2,i)  0   0           u(2,i)  2          -u(2,i)];
% end
% pinvM = pinv(M);
% Q = -N'*(M*pinvM-eye(2*n))*N;
% 
% Q11 = Q(1,1); Q12 = Q(1,2); Q13 = Q(1,3); Q14 = Q(1,4); Q15 = Q(1,5); Q16 = Q(1,6); Q17 = Q(1,7); Q18 = Q(1,8); Q19 = Q(1,9); Q110 = Q(1,10); 
%               Q22 = Q(2,2); Q23 = Q(2,3); Q24 = Q(2,4); Q25 = Q(2,5); Q26 = Q(2,6); Q27 = Q(2,7); Q28 = Q(2,8); Q29 = Q(2,9); Q210 = Q(2,10); 
%                             Q33 = Q(3,3); Q34 = Q(3,4); Q35 = Q(3,5); Q36 = Q(3,6); Q37 = Q(3,7); Q38 = Q(3,8); Q39 = Q(3,9); Q310 = Q(3,10); 
%                                           Q44 = Q(4,4); Q45 = Q(4,5); Q46 = Q(4,6); Q47 = Q(4,7); Q48 = Q(4,8); Q49 = Q(4,9); Q410 = Q(4,10); 
%                                                         Q55 = Q(5,5); Q56 = Q(5,6); Q57 = Q(5,7); Q58 = Q(5,8); Q59 = Q(5,9); Q510 = Q(5,10);
%                                                                       Q66 = Q(6,6); Q67 = Q(6,7); Q68 = Q(6,8); Q69 = Q(6,9); Q610 = Q(6,10);
%                                                                                     Q77 = Q(7,7); Q78 = Q(7,8); Q79 = Q(7,9); Q710 = Q(7,10);
%                                                                                                   Q88 = Q(8,8); Q89 = Q(8,9); Q810 = Q(8,10);
%                                                                                                                 Q99 = Q(9,9); Q910 = Q(9,10);
%                                                                                                                               Q1010 = Q(10,10);
% 
% c1 = [ 4*Q11, 6*Q12, 6*Q13, 6*Q14, 4*Q15 + 2*Q22, 4*Q16 + 4*Q23, 4*Q17 + 4*Q24, 4*Q18 + 2*Q33, 4*Q19 + 4*Q34, 4*Q110 + 2*Q44, 2*Q25, 2*Q26 + 2*Q35, 2*Q27 + 2*Q45, 2*Q28 + 2*Q36, 2*Q29 + 2*Q37 + 2*Q46, 2*Q210 + 2*Q47, 2*Q38, 2*Q39 + 2*Q48, 2*Q310 + 2*Q49, 2*Q410];
% c2 = [ 2*Q12, 4*Q15 + 2*Q22, 2*Q16 + 2*Q23, 2*Q17 + 2*Q24, 6*Q25, 4*Q26 + 4*Q35, 4*Q27 + 4*Q45, 2*Q28 + 2*Q36, 2*Q29 + 2*Q37 + 2*Q46, 2*Q210 + 2*Q47, 4*Q55, 6*Q56, 6*Q57, 4*Q58 + 2*Q66, 4*Q59 + 4*Q67, 4*Q510 + 2*Q77, 2*Q68, 2*Q69 + 2*Q78, 2*Q610 + 2*Q79, 2*Q710];
% c3 = [ 2*Q13, 2*Q16 + 2*Q23, 4*Q18 + 2*Q33, 2*Q19 + 2*Q34, 2*Q26 + 2*Q35, 4*Q28 + 4*Q36, 2*Q29 + 2*Q37 + 2*Q46, 6*Q38, 4*Q39 + 4*Q48, 2*Q310 + 2*Q49, 2*Q56, 4*Q58 + 2*Q66, 2*Q59 + 2*Q67, 6*Q68, 4*Q69 + 4*Q78, 2*Q610 + 2*Q79, 4*Q88, 6*Q89, 4*Q810 + 2*Q99, 2*Q910];
% c4 = [ 2*Q14, 2*Q17 + 2*Q24, 2*Q19 + 2*Q34, 4*Q110 + 2*Q44, 2*Q27 + 2*Q45, 2*Q29 + 2*Q37 + 2*Q46, 4*Q210 + 4*Q47, 2*Q39 + 2*Q48, 4*Q310 + 4*Q49, 6*Q410, 2*Q57, 2*Q59 + 2*Q67, 4*Q510 + 2*Q77, 2*Q69 + 2*Q78, 4*Q610 + 4*Q79, 6*Q710, 2*Q89, 4*Q810 + 2*Q99, 6*Q910, 4*Q1010];

%call the solver
%[x y z t] = GB_Solver_3Order_4Variable_UnitNorm_Division(c1, c2, c3, c4);

%using the Cayley form in DLS
p = U; z = u;
z_old = z; 
% make z into unit vectors from normalized pixel coords
z = [z; ones(1,size(z,2))];
z = z./ repmat(sqrt(sum(z.*z,1)),3,1);

% some preliminaries
flag = 0;
N = size(z,2);
 
% build coeff matrix
% An intermediate matrix, the inverse of what is called "H" in the paper
% (see eq. 25)
H = zeros(3);
for i = 1:N
    H = H + eye(3) - z(:,i)*z(:,i)';
end

A = zeros(3,9);
for i = 1:N
   A = A + (z(:,i)*z(:,i)' - eye(3)) * LeftMultVec(p(:,i));
end
A = H\A;

D = zeros(9);
for i = 1:N
    D = D + (LeftMultVec(p(:,i)) + A)' * (eye(3) - z(:,i)*z(:,i)') * (LeftMultVec(p(:,i)) + A);
end

c1 = [2*D(1,6) - 2*D(1,8) + 2*D(5,6) - 2*D(5,8) + 2*D(6,1) + 2*D(6,5) + 2*D(6,9) - 2*D(8,1) - 2*D(8,5) - 2*D(8,9) + 2*D(9,6) - 2*D(9,8); % constant term
          (6*D(1,2) + 6*D(1,4) + 6*D(2,1) - 6*D(2,5) - 6*D(2,9) + 6*D(4,1) - 6*D(4,5) - 6*D(4,9) - 6*D(5,2) - 6*D(5,4) - 6*D(9,2) - 6*D(9,4)); % s1^2  * s2
          (4*D(1,7) - 4*D(1,3) + 8*D(2,6) - 8*D(2,8) - 4*D(3,1) + 4*D(3,5) + 4*D(3,9) + 8*D(4,6) - 8*D(4,8) + 4*D(5,3) - 4*D(5,7) + 8*D(6,2) + 8*D(6,4) + 4*D(7,1) - 4*D(7,5) - 4*D(7,9) - 8*D(8,2) - 8*D(8,4) + 4*D(9,3) - 4*D(9,7)); % s1 * s2
          (4*D(1,2) - 4*D(1,4) + 4*D(2,1) - 4*D(2,5) - 4*D(2,9) + 8*D(3,6) - 8*D(3,8) - 4*D(4,1) + 4*D(4,5) + 4*D(4,9) - 4*D(5,2) + 4*D(5,4) + 8*D(6,3) + 8*D(6,7) + 8*D(7,6) - 8*D(7,8) - 8*D(8,3) - 8*D(8,7) - 4*D(9,2) + 4*D(9,4)); % s1 * s3
          (8*D(2,2) - 8*D(3,3) - 8*D(4,4) + 8*D(6,6) + 8*D(7,7) - 8*D(8,8)); % s2 * s3
          (4*D(2,6) - 2*D(1,7) - 2*D(1,3) + 4*D(2,8) - 2*D(3,1) + 2*D(3,5) - 2*D(3,9) + 4*D(4,6) + 4*D(4,8) + 2*D(5,3) + 2*D(5,7) + 4*D(6,2) + 4*D(6,4) - 2*D(7,1) + 2*D(7,5) - 2*D(7,9) + 4*D(8,2) + 4*D(8,4) - 2*D(9,3) - 2*D(9,7)); % s2^2 * s3
          (2*D(2,5) - 2*D(1,4) - 2*D(2,1) - 2*D(1,2) - 2*D(2,9) - 2*D(4,1) + 2*D(4,5) - 2*D(4,9) + 2*D(5,2) + 2*D(5,4) - 2*D(9,2) - 2*D(9,4)); % s2^3
          (4*D(1,9) - 4*D(1,1) + 8*D(3,3) + 8*D(3,7) + 4*D(5,5) + 8*D(7,3) + 8*D(7,7) + 4*D(9,1) - 4*D(9,9)); % s1 * s3^2
          (4*D(1,1) - 4*D(5,5) - 4*D(5,9) + 8*D(6,6) - 8*D(6,8) - 8*D(8,6) + 8*D(8,8) - 4*D(9,5) - 4*D(9,9)); % s1
          (2*D(1,3) + 2*D(1,7) + 4*D(2,6) - 4*D(2,8) + 2*D(3,1) + 2*D(3,5) + 2*D(3,9) - 4*D(4,6) + 4*D(4,8) + 2*D(5,3) + 2*D(5,7) + 4*D(6,2) - 4*D(6,4) + 2*D(7,1) + 2*D(7,5) + 2*D(7,9) - 4*D(8,2) + 4*D(8,4) + 2*D(9,3) + 2*D(9,7)); % s3
          (2*D(1,2) + 2*D(1,4) + 2*D(2,1) + 2*D(2,5) + 2*D(2,9) - 4*D(3,6) + 4*D(3,8) + 2*D(4,1) + 2*D(4,5) + 2*D(4,9) + 2*D(5,2) + 2*D(5,4) - 4*D(6,3) + 4*D(6,7) + 4*D(7,6) - 4*D(7,8) + 4*D(8,3) - 4*D(8,7) + 2*D(9,2) + 2*D(9,4)); % s2
          (2*D(2,9) - 2*D(1,4) - 2*D(2,1) - 2*D(2,5) - 2*D(1,2) + 4*D(3,6) + 4*D(3,8) - 2*D(4,1) - 2*D(4,5) + 2*D(4,9) - 2*D(5,2) - 2*D(5,4) + 4*D(6,3) + 4*D(6,7) + 4*D(7,6) + 4*D(7,8) + 4*D(8,3) + 4*D(8,7) + 2*D(9,2) + 2*D(9,4)); % s2 * s3^2
          (6*D(1,6) - 6*D(1,8) - 6*D(5,6) + 6*D(5,8) + 6*D(6,1) - 6*D(6,5) - 6*D(6,9) - 6*D(8,1) + 6*D(8,5) + 6*D(8,9) - 6*D(9,6) + 6*D(9,8)); % s1^2
          (2*D(1,8) - 2*D(1,6) + 4*D(2,3) + 4*D(2,7) + 4*D(3,2) - 4*D(3,4) - 4*D(4,3) - 4*D(4,7) - 2*D(5,6) + 2*D(5,8) - 2*D(6,1) - 2*D(6,5) + 2*D(6,9) + 4*D(7,2) - 4*D(7,4) + 2*D(8,1) + 2*D(8,5) - 2*D(8,9) + 2*D(9,6) - 2*D(9,8)); % s3^2
          (2*D(1,8) - 2*D(1,6) - 4*D(2,3) + 4*D(2,7) - 4*D(3,2) - 4*D(3,4) - 4*D(4,3) + 4*D(4,7) + 2*D(5,6) - 2*D(5,8) - 2*D(6,1) + 2*D(6,5) - 2*D(6,9) + 4*D(7,2) + 4*D(7,4) + 2*D(8,1) - 2*D(8,5) + 2*D(8,9) - 2*D(9,6) + 2*D(9,8)); % s2^2
          (2*D(3,9) - 2*D(1,7) - 2*D(3,1) - 2*D(3,5) - 2*D(1,3) - 2*D(5,3) - 2*D(5,7) - 2*D(7,1) - 2*D(7,5) + 2*D(7,9) + 2*D(9,3) + 2*D(9,7)); % s3^3
          (4*D(1,6) + 4*D(1,8) + 8*D(2,3) + 8*D(2,7) + 8*D(3,2) + 8*D(3,4) + 8*D(4,3) + 8*D(4,7) - 4*D(5,6) - 4*D(5,8) + 4*D(6,1) - 4*D(6,5) - 4*D(6,9) + 8*D(7,2) + 8*D(7,4) + 4*D(8,1) - 4*D(8,5) - 4*D(8,9) - 4*D(9,6) - 4*D(9,8)); % s1 * s2 * s3
          (4*D(1,5) - 4*D(1,1) + 8*D(2,2) + 8*D(2,4) + 8*D(4,2) + 8*D(4,4) + 4*D(5,1) - 4*D(5,5) + 4*D(9,9)); % s1 * s2^2
          (6*D(1,3) + 6*D(1,7) + 6*D(3,1) - 6*D(3,5) - 6*D(3,9) - 6*D(5,3) - 6*D(5,7) + 6*D(7,1) - 6*D(7,5) - 6*D(7,9) - 6*D(9,3) - 6*D(9,7)); % s1^2 * s3
          (4*D(1,1) - 4*D(1,5) - 4*D(1,9) - 4*D(5,1) + 4*D(5,5) + 4*D(5,9) - 4*D(9,1) + 4*D(9,5) + 4*D(9,9))].'; % s1^3

c2 = [- 2*D(1,3) + 2*D(1,7) - 2*D(3,1) - 2*D(3,5) - 2*D(3,9) - 2*D(5,3) + 2*D(5,7) + 2*D(7,1) + 2*D(7,5) + 2*D(7,9) - 2*D(9,3) + 2*D(9,7); % constant term
            (4*D(1,5) - 4*D(1,1) + 8*D(2,2) + 8*D(2,4) + 8*D(4,2) + 8*D(4,4) + 4*D(5,1) - 4*D(5,5) + 4*D(9,9)); % s1^2  * s2
            (4*D(1,8) - 4*D(1,6) - 8*D(2,3) + 8*D(2,7) - 8*D(3,2) - 8*D(3,4) - 8*D(4,3) + 8*D(4,7) + 4*D(5,6) - 4*D(5,8) - 4*D(6,1) + 4*D(6,5) - 4*D(6,9) + 8*D(7,2) + 8*D(7,4) + 4*D(8,1) - 4*D(8,5) + 4*D(8,9) - 4*D(9,6) + 4*D(9,8)); % s1 * s2
            (8*D(2,2) - 8*D(3,3) - 8*D(4,4) + 8*D(6,6) + 8*D(7,7) - 8*D(8,8)); % s1 * s3
            (4*D(1,4) - 4*D(1,2) - 4*D(2,1) + 4*D(2,5) - 4*D(2,9) - 8*D(3,6) - 8*D(3,8) + 4*D(4,1) - 4*D(4,5) + 4*D(4,9) + 4*D(5,2) - 4*D(5,4) - 8*D(6,3) + 8*D(6,7) + 8*D(7,6) + 8*D(7,8) - 8*D(8,3) + 8*D(8,7) - 4*D(9,2) + 4*D(9,4)); % s2 * s3
            (6*D(5,6) - 6*D(1,8) - 6*D(1,6) + 6*D(5,8) - 6*D(6,1) + 6*D(6,5) - 6*D(6,9) - 6*D(8,1) + 6*D(8,5) - 6*D(8,9) - 6*D(9,6) - 6*D(9,8)); % s2^2 * s3
            (4*D(1,1) - 4*D(1,5) + 4*D(1,9) - 4*D(5,1) + 4*D(5,5) - 4*D(5,9) + 4*D(9,1) - 4*D(9,5) + 4*D(9,9)); % s2^3
            (2*D(2,9) - 2*D(1,4) - 2*D(2,1) - 2*D(2,5) - 2*D(1,2) + 4*D(3,6) + 4*D(3,8) - 2*D(4,1) - 2*D(4,5) + 2*D(4,9) - 2*D(5,2) - 2*D(5,4) + 4*D(6,3) + 4*D(6,7) + 4*D(7,6) + 4*D(7,8) + 4*D(8,3) + 4*D(8,7) + 2*D(9,2) + 2*D(9,4)); % s1 * s3^2
            (2*D(1,2) + 2*D(1,4) + 2*D(2,1) + 2*D(2,5) + 2*D(2,9) - 4*D(3,6) + 4*D(3,8) + 2*D(4,1) + 2*D(4,5) + 2*D(4,9) + 2*D(5,2) + 2*D(5,4) - 4*D(6,3) + 4*D(6,7) + 4*D(7,6) - 4*D(7,8) + 4*D(8,3) - 4*D(8,7) + 2*D(9,2) + 2*D(9,4)); % s1
            (2*D(1,6) + 2*D(1,8) - 4*D(2,3) + 4*D(2,7) - 4*D(3,2) + 4*D(3,4) + 4*D(4,3) - 4*D(4,7) + 2*D(5,6) + 2*D(5,8) + 2*D(6,1) + 2*D(6,5) + 2*D(6,9) + 4*D(7,2) - 4*D(7,4) + 2*D(8,1) + 2*D(8,5) + 2*D(8,9) + 2*D(9,6) + 2*D(9,8)); % s3
            (8*D(3,3) - 4*D(1,9) - 4*D(1,1) - 8*D(3,7) + 4*D(5,5) - 8*D(7,3) + 8*D(7,7) - 4*D(9,1) - 4*D(9,9)); % s2
            (4*D(1,1) - 4*D(5,5) + 4*D(5,9) + 8*D(6,6) + 8*D(6,8) + 8*D(8,6) + 8*D(8,8) + 4*D(9,5) - 4*D(9,9)); % s2 * s3^2
            (2*D(1,7) - 2*D(1,3) + 4*D(2,6) - 4*D(2,8) - 2*D(3,1) + 2*D(3,5) + 2*D(3,9) + 4*D(4,6) - 4*D(4,8) + 2*D(5,3) - 2*D(5,7) + 4*D(6,2) + 4*D(6,4) + 2*D(7,1) - 2*D(7,5) - 2*D(7,9) - 4*D(8,2) - 4*D(8,4) + 2*D(9,3) - 2*D(9,7)); % s1^2
            (2*D(1,3) - 2*D(1,7) + 4*D(2,6) + 4*D(2,8) + 2*D(3,1) + 2*D(3,5) - 2*D(3,9) - 4*D(4,6) - 4*D(4,8) + 2*D(5,3) - 2*D(5,7) + 4*D(6,2) - 4*D(6,4) - 2*D(7,1) - 2*D(7,5) + 2*D(7,9) + 4*D(8,2) - 4*D(8,4) - 2*D(9,3) + 2*D(9,7)); % s3^2
            (6*D(1,3) - 6*D(1,7) + 6*D(3,1) - 6*D(3,5) + 6*D(3,9) - 6*D(5,3) + 6*D(5,7) - 6*D(7,1) + 6*D(7,5) - 6*D(7,9) + 6*D(9,3) - 6*D(9,7)); % s2^2
            (2*D(6,9) - 2*D(1,8) - 2*D(5,6) - 2*D(5,8) - 2*D(6,1) - 2*D(6,5) - 2*D(1,6) - 2*D(8,1) - 2*D(8,5) + 2*D(8,9) + 2*D(9,6) + 2*D(9,8)); % s3^3
            (8*D(2,6) - 4*D(1,7) - 4*D(1,3) + 8*D(2,8) - 4*D(3,1) + 4*D(3,5) - 4*D(3,9) + 8*D(4,6) + 8*D(4,8) + 4*D(5,3) + 4*D(5,7) + 8*D(6,2) + 8*D(6,4) - 4*D(7,1) + 4*D(7,5) - 4*D(7,9) + 8*D(8,2) + 8*D(8,4) - 4*D(9,3) - 4*D(9,7)); % s1 * s2 * s3
            (6*D(2,5) - 6*D(1,4) - 6*D(2,1) - 6*D(1,2) - 6*D(2,9) - 6*D(4,1) + 6*D(4,5) - 6*D(4,9) + 6*D(5,2) + 6*D(5,4) - 6*D(9,2) - 6*D(9,4)); % s1 * s2^2
            (2*D(1,6) + 2*D(1,8) + 4*D(2,3) + 4*D(2,7) + 4*D(3,2) + 4*D(3,4) + 4*D(4,3) + 4*D(4,7) - 2*D(5,6) - 2*D(5,8) + 2*D(6,1) - 2*D(6,5) - 2*D(6,9) + 4*D(7,2) + 4*D(7,4) + 2*D(8,1) - 2*D(8,5) - 2*D(8,9) - 2*D(9,6) - 2*D(9,8)); % s1^2 * s3
            (2*D(1,2) + 2*D(1,4) + 2*D(2,1) - 2*D(2,5) - 2*D(2,9) + 2*D(4,1) - 2*D(4,5) - 2*D(4,9) - 2*D(5,2) - 2*D(5,4) - 2*D(9,2) - 2*D(9,4))].'; % s1^3

c3 = [2*D(1,2) - 2*D(1,4) + 2*D(2,1) + 2*D(2,5) + 2*D(2,9) - 2*D(4,1) - 2*D(4,5) - 2*D(4,9) + 2*D(5,2) - 2*D(5,4) + 2*D(9,2) - 2*D(9,4); % constant term
          (2*D(1,6) + 2*D(1,8) + 4*D(2,3) + 4*D(2,7) + 4*D(3,2) + 4*D(3,4) + 4*D(4,3) + 4*D(4,7) - 2*D(5,6) - 2*D(5,8) + 2*D(6,1) - 2*D(6,5) - 2*D(6,9) + 4*D(7,2) + 4*D(7,4) + 2*D(8,1) - 2*D(8,5) - 2*D(8,9) - 2*D(9,6) - 2*D(9,8)); % s1^2  * s2
          (8*D(2,2) - 8*D(3,3) - 8*D(4,4) + 8*D(6,6) + 8*D(7,7) - 8*D(8,8)); % s1 * s2
          (4*D(1,8) - 4*D(1,6) + 8*D(2,3) + 8*D(2,7) + 8*D(3,2) - 8*D(3,4) - 8*D(4,3) - 8*D(4,7) - 4*D(5,6) + 4*D(5,8) - 4*D(6,1) - 4*D(6,5) + 4*D(6,9) + 8*D(7,2) - 8*D(7,4) + 4*D(8,1) + 4*D(8,5) - 4*D(8,9) + 4*D(9,6) - 4*D(9,8)); % s1 * s3
          (4*D(1,3) - 4*D(1,7) + 8*D(2,6) + 8*D(2,8) + 4*D(3,1) + 4*D(3,5) - 4*D(3,9) - 8*D(4,6) - 8*D(4,8) + 4*D(5,3) - 4*D(5,7) + 8*D(6,2) - 8*D(6,4) - 4*D(7,1) - 4*D(7,5) + 4*D(7,9) + 8*D(8,2) - 8*D(8,4) - 4*D(9,3) + 4*D(9,7)); % s2 * s3
          (4*D(1,1) - 4*D(5,5) + 4*D(5,9) + 8*D(6,6) + 8*D(6,8) + 8*D(8,6) + 8*D(8,8) + 4*D(9,5) - 4*D(9,9)); % s2^2 * s3
          (2*D(5,6) - 2*D(1,8) - 2*D(1,6) + 2*D(5,8) - 2*D(6,1) + 2*D(6,5) - 2*D(6,9) - 2*D(8,1) + 2*D(8,5) - 2*D(8,9) - 2*D(9,6) - 2*D(9,8)); % s2^3
          (6*D(3,9) - 6*D(1,7) - 6*D(3,1) - 6*D(3,5) - 6*D(1,3) - 6*D(5,3) - 6*D(5,7) - 6*D(7,1) - 6*D(7,5) + 6*D(7,9) + 6*D(9,3) + 6*D(9,7)); % s1 * s3^2
          (2*D(1,3) + 2*D(1,7) + 4*D(2,6) - 4*D(2,8) + 2*D(3,1) + 2*D(3,5) + 2*D(3,9) - 4*D(4,6) + 4*D(4,8) + 2*D(5,3) + 2*D(5,7) + 4*D(6,2) - 4*D(6,4) + 2*D(7,1) + 2*D(7,5) + 2*D(7,9) - 4*D(8,2) + 4*D(8,4) + 2*D(9,3) + 2*D(9,7)); % s1
          (8*D(2,2) - 4*D(1,5) - 4*D(1,1) - 8*D(2,4) - 8*D(4,2) + 8*D(4,4) - 4*D(5,1) - 4*D(5,5) + 4*D(9,9)); % s3
          (2*D(1,6) + 2*D(1,8) - 4*D(2,3) + 4*D(2,7) - 4*D(3,2) + 4*D(3,4) + 4*D(4,3) - 4*D(4,7) + 2*D(5,6) + 2*D(5,8) + 2*D(6,1) + 2*D(6,5) + 2*D(6,9) + 4*D(7,2) - 4*D(7,4) + 2*D(8,1) + 2*D(8,5) + 2*D(8,9) + 2*D(9,6) + 2*D(9,8)); % s2
          (6*D(6,9) - 6*D(1,8) - 6*D(5,6) - 6*D(5,8) - 6*D(6,1) - 6*D(6,5) - 6*D(1,6) - 6*D(8,1) - 6*D(8,5) + 6*D(8,9) + 6*D(9,6) + 6*D(9,8)); % s2 * s3^2
          (2*D(1,2) - 2*D(1,4) + 2*D(2,1) - 2*D(2,5) - 2*D(2,9) + 4*D(3,6) - 4*D(3,8) - 2*D(4,1) + 2*D(4,5) + 2*D(4,9) - 2*D(5,2) + 2*D(5,4) + 4*D(6,3) + 4*D(6,7) + 4*D(7,6) - 4*D(7,8) - 4*D(8,3) - 4*D(8,7) - 2*D(9,2) + 2*D(9,4)); % s1^2
          (6*D(1,4) - 6*D(1,2) - 6*D(2,1) - 6*D(2,5) + 6*D(2,9) + 6*D(4,1) + 6*D(4,5) - 6*D(4,9) - 6*D(5,2) + 6*D(5,4) + 6*D(9,2) - 6*D(9,4)); % s3^2
          (2*D(1,4) - 2*D(1,2) - 2*D(2,1) + 2*D(2,5) - 2*D(2,9) - 4*D(3,6) - 4*D(3,8) + 2*D(4,1) - 2*D(4,5) + 2*D(4,9) + 2*D(5,2) - 2*D(5,4) - 4*D(6,3) + 4*D(6,7) + 4*D(7,6) + 4*D(7,8) - 4*D(8,3) + 4*D(8,7) - 2*D(9,2) + 2*D(9,4)); % s2^2
          (4*D(1,1) + 4*D(1,5) - 4*D(1,9) + 4*D(5,1) + 4*D(5,5) - 4*D(5,9) - 4*D(9,1) - 4*D(9,5) + 4*D(9,9)); % s3^3
          (4*D(2,9) - 4*D(1,4) - 4*D(2,1) - 4*D(2,5) - 4*D(1,2) + 8*D(3,6) + 8*D(3,8) - 4*D(4,1) - 4*D(4,5) + 4*D(4,9) - 4*D(5,2) - 4*D(5,4) + 8*D(6,3) + 8*D(6,7) + 8*D(7,6) + 8*D(7,8) + 8*D(8,3) + 8*D(8,7) + 4*D(9,2) + 4*D(9,4)); % s1 * s2 * s3
          (4*D(2,6) - 2*D(1,7) - 2*D(1,3) + 4*D(2,8) - 2*D(3,1) + 2*D(3,5) - 2*D(3,9) + 4*D(4,6) + 4*D(4,8) + 2*D(5,3) + 2*D(5,7) + 4*D(6,2) + 4*D(6,4) - 2*D(7,1) + 2*D(7,5) - 2*D(7,9) + 4*D(8,2) + 4*D(8,4) - 2*D(9,3) - 2*D(9,7)); % s1 * s2^2
          (4*D(1,9) - 4*D(1,1) + 8*D(3,3) + 8*D(3,7) + 4*D(5,5) + 8*D(7,3) + 8*D(7,7) + 4*D(9,1) - 4*D(9,9)); % s1^2 * s3
          (2*D(1,3) + 2*D(1,7) + 2*D(3,1) - 2*D(3,5) - 2*D(3,9) - 2*D(5,3) - 2*D(5,7) + 2*D(7,1) - 2*D(7,5) - 2*D(7,9) - 2*D(9,3) - 2*D(9,7))].'; % s1^3

end

function M = LeftMultVec(v)
% R * p = LeftMultVec(p) * vec(R)
M = [v' zeros(1,6);
     zeros(1,3) v' zeros(1,3);
     zeros(1,6) v'];
end